using System;
using System.Collections.Generic;
using System.Text;
using System.Xml.Serialization;
using System.IO;

namespace Blimpbots.Recognition
{
    [Serializable]
    public struct CameraProperties
    {
        public double[][] intrinsicMatrix; // 3x3
        public double[][] rotationMatrix;  // 3x3
        public double[] translationVector; // 3
        public double[] distortion; // 4

        #region Intrinsic Property Accessors
        public double FX { get { return intrinsicMatrix[0][0]; } }
        public double FY { get { return intrinsicMatrix[1][1]; } }
        public double CX { get { return intrinsicMatrix[0][2]; } }
        public double CY { get { return intrinsicMatrix[1][2]; } }
        #endregion

        // Roll around Z
        public double Roll
        {
            get {
                return Math.Atan2(rotationMatrix[0][1], 
                                  rotationMatrix[0][0]);
            }
        }
        // Pitch around Y
        public double Pitch
        {
            get {
                return -Math.Asin(rotationMatrix[0][2]);
            }
        }
        // Yaw around X
        public double Yaw
        {
            get {
                return Math.Atan2(rotationMatrix[1][2], 
                                  rotationMatrix[2][2]);
            }
        }

        #region Serialization Utilities
        public static void SaveTo(Stream s, CameraProperties prop)
        {
            XmlSerializer xs = new XmlSerializer(typeof(CameraProperties));
            xs.Serialize(s, prop);
        }
        public static CameraProperties ReadFrom(Stream s)
        {
            XmlSerializer xs = new XmlSerializer(typeof(CameraProperties));
            CameraProperties result = (CameraProperties)xs.Deserialize(s);
            return result;
        }
        #endregion

    }
}
